/*
 * Copyright (c) 2011, Fredrik Häggström
 *
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Luleå University of Technology nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <tT.h>
#include <env.h>

#include "mcpwm.h"

#define MCPWM_PERIOD 2048		// Switching frequency = CCLK / MCPWM_PERIOD

#define MCPWM_ENABLE 1
extern uint32_t mode;

env_result_t mcpwm_init(mcpwm_t *self, void *none)
{
	ENV_DEBUG("enter init_mcpwm()\r\n");

	/* Drive ports LOW UM p.126*/
	GPIO1->FIOCLR |= (1<<19) | (1<<22) | (1<<25) | (1<<26) | (1<<28) | (1<<29);
	
	/* Set ports to output when in general mode p.123 */
	//				  //MCOA0   //MCOB0   //MCOA1  //MCOB1   //MCOA2   //MCOB2
	GPIO1->FIODIR |= (1<<19) | (1<<22) | (1<<25) | (1<<26) | (1<<28) | (1<<29);	

	PINCON->PINMODE3 |=	(3<<10); 	//Pull down resistor
	//PINCON->PINSEL3 |= 	(1<<10); 	//MCABORT
	
	/* Power up the MCPWM unit */	
	SC->PCONP |= (1<<17);
	
	/* Clock select 00 = CCLK/4 01 = CCLK 10 = CCLk/2 11 = CCLK/8	*/
	SC->PCLKSEL1 &= ~(3<<30);
	SC->PCLKSEL1 |= (1<<30);	
;
	/**
	*  Pin function select see UM p.109
	*  Enable only the inputs. The outputs must me enabled by enable_mcpwm() 
	PINCON->PINSEL3 |= 	//(1<<6) | 	//MCOA0
			//	(1<<8) | 	//MCI0
			//	(1<<10) | 	//MCABORT not available on 80-pin package
			//	(1<<12) | 	//MCOB0
			//	(1<<14) | 	//MCI1
			//	(1<<16)  	//MCI2
			//	(1<<18) | 	//MCOA1
			//	(1<<20) | 	//MC0B1
			//	(1<<24) | 	//MCOA2
			//	(1<<26);   	//MCOB2
	*/			
		
	/* Motor Control Register see UM p.526 */
	MCPWM->MCCON_SET = 	
				(0<<1) | 	//edge-aligned ch0
				(0<<2) | 	//passive = LOW, active = HIGH ch0
				(1<<3) | 	//enable Dead-time ch0
				(0<<9) | 	//edge-aligned ch1
	/*One DC motor*/	(0<<10) | 	//passive = LOW, active = HIGH ch1
				(1<<11) | 	//enable Dead-time ch1
				(0<<17) | 	//edge-aligned ch2
				(0<<18) | 	//passive = LOW, active = HIGH ch2
				(1<<19) | 	//enable Dead-time ch2
				(1<<30); 	//enable AC mode. All timers run from TC0
	

	/* Motor Counter Control Register see UM p.531 */
	MCPWM->MCCNTCON_CLR = 	(1<<29) | 	//counter0 timer mode
				(1<<30) | 	//counter1 timer mode
				(1<<31);	//counter2 timer mode

	
	/* Counter Limit values */
	MCPWM->MCPER0 = MCPWM_PERIOD;	//TOP value = 2048 -> 48kHz freq @ 100MHz
	MCPWM->MCPER1 = MCPWM_PERIOD;
	MCPWM->MCPER2 = MCPWM_PERIOD;

	/* Counter Match values */
	MCPWM->MCPW0 = MCPWM_PERIOD/2;	//MAT value = 1024 -> 50% duty cycle
	MCPWM->MCPW1 = MCPWM_PERIOD/2;
	MCPWM->MCPW2 = MCPWM_PERIOD/2;
	
	/****************** Dead-time Register see UM p.535 ***********************
	* Maximum turn-off delay time minus maximum turn on time on the transistor
	* IRFB_S_SL3306: Off time delay = 40ns, On time delay = 15ns. 
	* @ 100MHz one cykel = 10 ns 
	* 40 - 15 = 25ns -> 3 cycles
	**************************************************************************/
	MCPWM->MCDEADTIME = (10<<0) | (10<<10) | (10<<20);	//set 300ns Dead-time for all 3 channels
	
	/** Is not working. See erratasheet.
	
	// Pin input mode. Disable internal pull-up resistor see UM p.114 
	PINCON->PINMODE3 |=	(2<<8) | 	//MCI0
				(2<<14) | 	//MCI1
				(2<<16);	//MCI2
	
	// Motor Capture Register see UM p.528
	MCPWM->MCCAPCON_SET =	(1<<0) | 	//event on ICAP0 for rising on MCI0
				(1<<8) | 	//event on ICAP1 for rising on MCI1
				(1<<16) | 	//event on ICAP2 for rising on MCI2
				(7<<21);	//hardware noise filtering on MCI0-2
	
	//Clear Input cap. flags 
	MCPWM->MCINTFLAG_CLR = 	(1<<0) | 	//flag ILIM0
				(1<<2) | 	//flag ICAP0
				(1<<6) | 	//flag ICAP1
				(1<<10);	//flag ICAP2

	//Motor Interupt Enable Register see UM p.529 
	MCPWM->MCINTEN_SET = 	(0<<0) | 	//flag ILIM0
				(1<<2) | 	//ICAP0
				(1<<6) | 	//ICAP1
				(1<<10);	//ICAP2
	*/
	
	/* Stop MCPWM timer 0 and reset it */
	
	MCPWM->MCCON_CLR 	= (1<<0) | (1<<8) | (1<<16);
	MCPWM->MCTIM0 		= 0;
	MCPWM->MCTIM1 		= 0;
	MCPWM->MCTIM2 		= 0;
	MCPWM->MCCON_SET 	= (1<<0) | (1<<8) | (1<<16);
	
	//MCPWM->MCINTEN_SET = 	(1<<15); // Enable MCABORT interrupt
	
	//NVIC_EnableIRQ(MCPWM_IRQn);
	
	//NVIC_SetPriority(MCPWM_IRQn, 3);
	

	
	
	return 0;
}

env_result_t mcpwm_enable(mcpwm_t *self, void *none)
{
	//ENV_DEBUG("enter enable_mcpwm()\r\n");

	if (!(self->status & MCPWM_ENABLE))
	{
		self->status |= MCPWM_ENABLE;		
		
		/* Pin function select see UM p.109 
		*  Set pins to MCPMW functions 	*/
		PINCON->PINSEL3 |= //(1<<6) |	//MCOA0
				//(1<<8) | 	//MCI0
				//(1<<10) | 	//MCABORT not available on 80-pin package
				//(1<<12) | 	//MCOB0
				//(1<<14) | 	//MCI1
				//(1<<16) | 	//MCI2
				(1<<18) | 	//MCOA1
				(1<<20) |	//MC0B1
				(1<<24) |	//MCOA2
				(1<<26)   	//MCOB2
				;
				
				
		CUR_RESET();
	}
	
	return 0;
}

env_result_t mcpwm_reset_clear(mcpwm_t *self, void *none)
{
  
  //ENV_DEBUG("enter mcpwm_reset_clear()\r\n");
  //set as input so hardware can draw the pin high if needed.
  GPIO1->FIODIR &=  ~(1<<20);  	//CURRESET
  return 0;
  
}

env_result_t mcpwm_disable(mcpwm_t *self, void *none)
{
	//ENV_DEBUG("enter disable_mcpwm()\r\n");

	if (self->status & MCPWM_ENABLE)
	{
		self->status &= !(MCPWM_ENABLE);		
		
		/* Pin function select see UM p.109 
		*  set output pins to GPIO 	*/
		PINCON->PINSEL3 &= 	~((1<<6) | 	//MCOA0
				//	(1<<8) | 	//MCI0
				//	(1<<10) | 	//MCABORT not available on 80-pin package
					(1<<12) | 	//MCOB0
				//	(1<<14) | 	//MCI1
				//	(1<<16) | 	//MCI2
					//(1<<18) | 	//MCOA1
					(1<<20) | 	//MC0B1
					(1<<24) | 	//MCOA2
					(1<<26))   	//MCOB2
					;
		
		/* Drive ports LOW UM p.126*/
		GPIO1->FIOCLR |=	(1<<19) | 
					(1<<22) | 
					(1<<25) | 
					(1<<26) | 
					(1<<28) | 
					(1<<29);
		mode = 2;
		//d_delay(1000);
		//v_leg(MC_LOW);
	}
	
	return 0;
}

env_result_t mcpwm_set_duty(mcpwm_t *self, int32_t *val)
{
	//ENV_DEBUG("enter set_duty()\r\n");
	
	//env_send(*val);
	if ( mode != 2 ) mcpwm_enable(self, TT_ARGS_NONE);
	
	if ( self->status & MCPWM_ENABLE ) {
	
		if ( *val > -1024 && *val < 1024) {
			
			self->duty = *val; 
		
			MCPWM->MCPW0 = 1024 - *val;	//MAT value = 2048 -> 50% duty cycle
			MCPWM->MCPW1 = 1024 - *val;	
			MCPWM->MCPW2 = 1024 - *val;
			//env_send(self->duty);	
			if (  *val == 0 && mode == 2)
			{	

				/* switch off transistors if wanted duty 0 */
				mcpwm_disable(self, TT_ARGS_NONE);
			}
			else
			{
				mcpwm_enable(self, TT_ARGS_NONE);
			}
		}
		else
		{
			mcpwm_disable(self, TT_ARGS_NONE);
			return 1;	//ERROR Invalid value or status is disabled
		}
	}
	else {
		mcpwm_disable(self, TT_ARGS_NONE);
	}
	return 0;
}



env_result_t mcpwm_decode(mcpwm_t *self, uint32_t data[2]) 
{
	ENV_DEBUG("mcpwm_decode() ");
	switch (data[0] & 0x000000FF) {
	
		case 0: ENV_DEBUG("duty ");
						mode = 2;
						mcpwm_enable(self,TT_ARGS_NONE);
						mcpwm_set_duty(self, (int32_t*)&(data[1]));
						break;
	
		case 1:	ENV_DEBUG("dt ");
						mcpwm_set_dt(self, (uint32_t*)&(data[1]));
						break;
																	
		default:
						break;
	}	
	env_send(data[1]);
	ENV_DEBUG("\r\n");

	
	
	return 0;
}

env_result_t mcpwm_set_dt(mcpwm_t *self, uint32_t *val)
{
	ENV_DEBUG("enter set_dt()\r\n");
	
	if ( *val < 1024 )
	{
		self->dt = *val;
		//MCPWM->MCDEADTIME = (*val<<0) | (*val<<10) | (*val<<20);
		
	}
	else
	{
		return 1;	//ERROR Invalid value
	}
	
	return 0;
} 

void d_delay(uint32_t count)
{
	uint32_t i;
	volatile uint32_t temp = count / 6;
	for (i = 0; i < temp ; i++)
		{
			;//Count
		} 
}

void u_leg(int32_t arg)
{
	if ( arg == MC_HIGH ) {
		  
		if ( GPIO1->FIOPIN & (1<<22)) {
			GPIO1->FIOCLR =	(1<<22);
			d_delay(MC_DEADTIME);
		}
		GPIO1->FIOSET = (1<<19);
	}
	else if (arg == MC_LOW) {
	
		if ( GPIO1->FIOPIN & (1<<19)) {
			GPIO1->FIOCLR =	(1<<19);
			d_delay(MC_DEADTIME);
		}
		GPIO1->FIOSET = (1<<22);
	}
	else {
	
		GPIO1->FIOCLR =	(1<<19) | (1<<22);
	
	}
}

void v_leg(int32_t arg)
{
	if ( arg == MC_HIGH ) {
		  
		if ( GPIO1->FIOPIN & (1<<25)) {
			GPIO1->FIOCLR =	(1<<25);
			d_delay(MC_DEADTIME);
		}
		GPIO1->FIOSET = (1<<26);
	}
	else if (arg == MC_LOW ){
		
		if ( GPIO1->FIOPIN & (1<<26)) {
			GPIO1->FIOCLR =	(1<<26);
			d_delay(MC_DEADTIME);
		}
		GPIO1->FIOSET = (1<<25);
	}
	else {
		
		GPIO1->FIOCLR = (1<<25) | (1<<26);
	
	}
}

void w_leg(int32_t arg)
{
	if ( arg == MC_HIGH ) {
		  
		if ( GPIO1->FIOPIN & (1<<29)) {
			GPIO1->FIOCLR =	(1<<29);
			d_delay(MC_DEADTIME);
		}
		GPIO1->FIOSET = (1<<28);
	}
	else if ( arg == MC_LOW ) {
	
		if ( GPIO1->FIOPIN & (1<<28)) {
			GPIO1->FIOCLR =	(1<<28);
			d_delay(MC_DEADTIME);
		}
		GPIO1->FIOSET = (1<<29);
	}
	else {
	
		GPIO1->FIOCLR = (1<<28) | (1<<29);
		
	}
}


void MCPWM_IRQHandler(void)
{
  /** Not working. See erratasheet
	// ILIM0 Interrupt 
	if (MCPWM->MCINTFLAG & (1<<0))
	{
		MCPWM->MCINTFLAG_CLR = (1<<0);
		mcpwm.counter++;
	}
	

	// ICAP0 Interrupt 	
	if (MCPWM->MCINTFLAG & (1<<2))
	{
		MCPWM->MCINTFLAG_CLR = (1<<2);
		ENV_DEBUG("INPUT0\r\n");
		// Rising 
		if (MCPWM->MCCAPCON & (1<<0))
		{
			mcpwm_capture( 0 & (1<<2));
			MCPWM->MCCAPCON_CLR = (1<<0);
			MCPWM->MCCAPCON_SET = (1<<1);
		}
		// Falling 
		else
		{
			mcpwm_capture( 0 & (0<<2));
			MCPWM->MCCAPCON_CLR = (1<<1);
			MCPWM->MCCAPCON_SET = (1<<0);
		}
	}	
	
	// ICAP1 Interrupt 	
	if (MCPWM->MCINTFLAG & (1<<6))
	{
		MCPWM->MCINTFLAG_CLR = (1<<6);
		ENV_DEBUG("INPUT1\r\n");
		// Rising
		if (MCPWM->MCCAPCON & (1<<8))
		{
			mcpwm_capture( 1 & (1<<2));
			MCPWM->MCCAPCON_CLR = (1<<8);
			MCPWM->MCCAPCON_SET = (1<<9);
		}
		// Falling 
		else
		{
			mcpwm_capture( 1 & (0<<2));
			MCPWM->MCCAPCON_CLR = (1<<9);
			MCPWM->MCCAPCON_SET = (1<<8);
		}
	}

	// ICAP2 Interrupt 	
	if (MCPWM->MCINTFLAG & (1<<10))
	{
		MCPWM->MCINTFLAG_CLR = (1<<10);
		ENV_DEBUG("INPUT2\r\n");
		// Rising 
		if (MCPWM->MCCAPCON & (1<<16))
		{
			mcpwm_capture( 2 & (1<<2));
			MCPWM->MCCAPCON_CLR = (1<<16);
			MCPWM->MCCAPCON_SET = (1<<17);
		}
		// Falling 
		else
		{
			mcpwm_capture( 2 & (0<<2));
			MCPWM->MCCAPCON_CLR = (1<<17);
			MCPWM->MCCAPCON_SET = (1<<16);
		}
	}		
	**/
	if (MCPWM->MCINTFLAG & (1<<15)) {
		ENV_DEBUG("ABORT!!!\r\n");
		mcpwm.status = 0;
		mcpwm_disable(&mcpwm, TT_ARGS_NONE);
	}
	
	MCPWM->MCINTFLAG_CLR = 0xFFFFFFFF;
	// Throw in some nops so the interrupt handler is not taken again.
	__NOP();
	__NOP();
	__NOP();
}


